package com.rgbgame.game;// Holder for vectors, vertices, or RGB colors (I should have just used a
// color class, but it's too late now)

// Has exclusive use of global buffer vectors bufferC and bufferD
public final class Vector3d {
	private static final Vector3d bufferA = new Vector3d(0, 0, 0);
	private static final Vector3d bufferB = new Vector3d(0, 0, 0);
	private static final Vector3d bufferC = new Vector3d(0, 0, 0);
	private static final Vector3d bufferD = new Vector3d(0, 0, 0);
	public float x;
	public float y;
	public float z;

	public Vector3d(float myX, float myY, float myZ) {
		// debugCounter++;
		x = myX;
		y = myY;
		z = myZ;
	}

	public Vector3d(Vector3d _vec) {
		// debugCounter++;
		x = _vec.x;
		y = _vec.y;
		z = _vec.z;
	}

	public Vector3d copy() {
		// debugCounter++;
		return new Vector3d(x, y, z);
	}

	public Vector3d plus(Vector3d addMe) {
		// debugCounter++;
		return new Vector3d(x + addMe.x, y + addMe.y, z + addMe.z);
	}

	public void set(float _x, float _y, float _z) {
		x = _x;
		y = _y;
		z = _z;
	}

	public void set(Vector3d vec) {
		x = vec.x;
		y = vec.y;
		z = vec.z;
	}

	public void add(Vector3d addMe) {
		x += addMe.x;
		y += addMe.y;
		z += addMe.z;
	}

	public void subtract(Vector3d subMe) {
		x -= subMe.x;
		y -= subMe.y;
		z -= subMe.z;
	}

	public void multiply(float multMe) {
		x *= multMe;
		y *= multMe;
		z *= multMe;
	}

	public Vector3d minus(Vector3d addMe) {
		// debugCounter++;
		return new Vector3d(x - addMe.x, y - addMe.y, z - addMe.z);
	}

	public Vector3d times(float multMe) {
		// debugCounter++;
		return new Vector3d(x * multMe, y * multMe, z * multMe);
	}

	public float norm() {
		return (float) Math.sqrt(x * x + y * y + z * z);
	}

	public float normSquared() {
		return (x * x + y * y + z * z);
	}

	public float dot(Vector3d dotMe) {
		return (x * dotMe.x + y * dotMe.y + z * dotMe.z);
	}

	public void normalize() {
		// debugCounter++;
		float tnorm = this.norm();
		if (tnorm < .0001f) {
			// println("Zero norm");
			// Screw it, we can't do anything reasonable here
			// Maybe we should make this a boolean function, return false if
			// we get here?
			return;
		}
		float myNormI = 1.0f / tnorm;
		x *= myNormI;
		y *= myNormI;
		z *= myNormI;
	}

	public Vector3d normalized() {
		// debugCounter++;
		Vector3d result = new Vector3d(this);
		result.normalize();
		return result;
	}

	public Vector3d direction() {
		// debugCounter++;
		float myNorm = this.norm();
		return new Vector3d(x / myNorm, y / myNorm, z / myNorm);
	}

	public float distanceTo(Vector3d _vec) {
		return (float) Math.sqrt((x - _vec.x) * (x - _vec.x) + (y - _vec.y)
				* (y - _vec.y) + (z - _vec.z) * (z - _vec.z));
	}

	public float distanceSqr(Vector3d _vec) {
		return (x - _vec.x) * (x - _vec.x) + (y - _vec.y) * (y - _vec.y)
				+ (z - _vec.z) * (z - _vec.z);
	}

	public Vector3d directionTo(Vector3d _vec) {
		// debugCounter++;
		Vector3d result = new Vector3d(_vec.x - x, _vec.y - y, _vec.z - z);
		result.normalize();
		return result;
	}

	public Vector3d cross(Vector3d crossMe) {
		// debugCounter++;
		return new Vector3d((y * crossMe.z - z * crossMe.y), (z * crossMe.x - x
				* crossMe.z), (x * crossMe.y - y * crossMe.x));
	}

	public Vector3d projectAlong(Vector3d alongMe) {
		Vector3d buffer = alongMe.normalized();
		buffer.multiply(this.dot(buffer));
		return buffer;
	}

	/*
	 * To inline projectAlong: float invMyNorm = 1.0/alongMe.norm(); float bx =
	 * alongMe.x * invMyNorm; float by = alongMe.y * invMyNorm; float thisdotb =
	 * this.x*bx+this.y*by; float resultX = bx*thisdotb; float resultY =
	 * by*thisdotb;
	 */

	public Vector3d reflectOver(Vector3d lineStart, Vector3d lineEnd) {
		/*
		 * com.rgbgame.game.Vector3d diff = lineEnd.minus(lineStart); com.rgbgame.game.Vector3d diff2 =
		 * lineEnd.minus(this); com.rgbgame.game.Vector3d proj = diff2.projectAlong(diff);
		 * com.rgbgame.game.Vector3d perp = diff2.minus(proj); return this.plus(perp.times(2.0));
		 */
		return reflectOver(lineStart, lineEnd, 1.0f);
	}

	public Vector3d reflectOver(Vector3d lineStart, Vector3d lineEnd,
			float perpCoef) {
		// Same as above, but scales perpendicular distance by perpCoef (1
		// is a perfect reflection, 0 moves it to line, -1 doesn't move it
		// at all) - useful for collisions
		Vector3d diff = lineEnd.minus(lineStart);
		Vector3d diff2 = lineEnd.minus(this);
		Vector3d proj = diff2.projectAlong(diff);
		Vector3d perp = diff2.minus(proj);
		return this.plus(perp.times(1.0f + perpCoef));

		// Version with temp com.rgbgame.game.Vector3d creation eliminated - disabled until
		// can be tested
		/*
		 * bufferC.set(lineEnd); bufferC.subtract(lineStart);
		 * bufferD.set(lineEnd); bufferD.subtract(this); float invMyNorm =
		 * 1.0/bufferC.norm(); float bx = bufferC.x * invMyNorm; float by =
		 * bufferC.y * invMyNorm; float thisdotb = bufferD.x*bx+bufferD.y*by;
		 * bufferC.set(bx*thisdotb,by*thisdotb,0.0); bufferD.subtract(bufferC);
		 * bufferD.multiply(1.0+perpCoef); return this.plus(bufferD);
		 */
	}

	/*
	 * 2d distance (in xy plane) between vectors
	 */
	public static float distance2d(Vector3d v1, Vector3d v2) {
		float dx = v1.x - v2.x;
		float dy = v1.y - v2.y;
		return (float) Math.sqrt(dx * dx + dy * dy);
	}

	public static float distance2d(float v1x, float v1y, float v2x, float v2y) {
		float dx = v1x - v2x;
		float dy = v1y - v2y;
		return (float) Math.sqrt(dx * dx + dy * dy);
	}

	public float distanceToLine(Vector3d lineStart, Vector3d lineEnd) {
		/*
		 * com.rgbgame.game.Vector3d diff = lineEnd.minus(lineStart); com.rgbgame.game.Vector3d diff2 =
		 * lineEnd.minus(this); com.rgbgame.game.Vector3d proj = diff2.projectAlong(diff);
		 * com.rgbgame.game.Vector3d perp = diff2.minus(proj); return perp.norm();
		 */

		float vx = lineEnd.x - lineStart.x;
		float vy = lineEnd.y - lineStart.y;
		float wx = x - lineStart.x;
		float wy = y - lineStart.y;
		double c1 = vx * wx + vy * wy;
		if (c1 <= 0)
			return distance2d(x, y, lineStart.x, lineStart.y);
		double c2 = vx * vx + vy * vy;
		if (c2 <= c1)
			return distance2d(x, y, lineEnd.x, lineEnd.y);
		double b = c1 / c2;
		float px = lineStart.x + (float) (b * vx);
		float py = lineStart.y + (float) (b * vy);
		return distance2d(x, y, px, py);
	}

	public Vector3d displacementToLine(Vector3d lineStart, Vector3d lineEnd) {
		/*
		 * com.rgbgame.game.Vector3d diff = lineEnd.minus(lineStart); com.rgbgame.game.Vector3d diff2 =
		 * lineEnd.minus(this); com.rgbgame.game.Vector3d proj = diff2.projectAlong(diff); return
		 * diff2.minus(proj);
		 */

		// Clean as it gets
		float vx = lineEnd.x - lineStart.x;
		float vy = lineEnd.y - lineStart.y;
		float wx = x - lineStart.x;
		float wy = y - lineStart.y;
		double c1 = vx * wx + vy * wy;
		if (c1 <= 0)
			return new Vector3d(lineStart.x - x, lineStart.y - y, 0f);
		double c2 = vx * vx + vy * vy;
		if (c2 <= c1)
			return new Vector3d(lineEnd.x - x, lineEnd.y - y, 0f);
		double b = c1 / c2;
		float px = lineStart.x + (float) (b * vx);
		float py = lineStart.y + (float) (b * vy);
		return new Vector3d(px - x, py - y, 0f);

		/*
		 * //Version with temp com.rgbgame.game.Vector3d creation eliminated
		 * bufferC.set(lineEnd); bufferC.subtract(lineStart);
		 * bufferD.set(lineEnd); bufferD.subtract(this); float cnorm =
		 * bufferC.norm(); if (cnorm < .0001f) { return new com.rgbgame.game.Vector3d(0, 0, 0); }
		 * float invMyNorm = 1.0f / cnorm; float bx = bufferC.x * invMyNorm;
		 * float by = bufferC.y * invMyNorm; float thisdotb = bufferD.x * bx +
		 * bufferD.y * by; bufferC.set(bx * thisdotb, by * thisdotb, 0.0f);
		 * bufferD.subtract(bufferC); // debugCounter++; return new
		 * com.rgbgame.game.Vector3d(bufferD);
		 */
	}

	public void displacementToLineSet(Vector3d lineStart, Vector3d lineEnd,
			Vector3d result) {
		// Same as displacementToLine, but this version just loads the result
		// into result instead of creating new vector
		/*
		 * bufferC.set(lineEnd); bufferC.subtract(lineStart);
		 * bufferD.set(lineEnd); bufferD.subtract(this); float cnorm =
		 * bufferC.norm(); if (cnorm < .0001f) { result.set(0, 0, 0); return; }
		 * float invMyNorm = 1.0f / cnorm; float bx = bufferC.x * invMyNorm;
		 * float by = bufferC.y * invMyNorm; float thisdotb = bufferD.x * bx +
		 * bufferD.y * by; bufferC.set(bx * thisdotb, by * thisdotb, 0.0f);
		 * bufferD.subtract(bufferC); // debugCounter++; result.set(bufferD);
		 */

		float vx = lineEnd.x - lineStart.x;
		float vy = lineEnd.y - lineStart.y;
		float wx = x - lineStart.x;
		float wy = y - lineStart.y;
		double c1 = vx * wx + vy * wy;
		if (c1 <= 0) {
			result.set(lineStart.x - x, lineStart.y - y, 0f);
			return;
		}
		double c2 = vx * vx + vy * vy;
		if (c2 <= c1) {
			result.set(lineEnd.x - x, lineEnd.y - y, 0f);
			return;
		}
		double b = c1 / c2;
		float px = lineStart.x + (float) (b * vx);
		float py = lineStart.y + (float) (b * vy);
		result.set(px - x, py - y, 0f);
		return;
	}

	public void closestPointOnLineSet(Vector3d lineStart, Vector3d lineEnd,
			Vector3d result) {
		float vx = lineEnd.x - lineStart.x;
		float vy = lineEnd.y - lineStart.y;
		float wx = x - lineStart.x;
		float wy = y - lineStart.y;
		double c1 = vx * wx + vy * wy;
		if (c1 <= 0) {
			result.set(lineStart.x, lineStart.y, 0f);
			return;
		}
		double c2 = vx * vx + vy * vy;
		if (c2 <= c1) {
			result.set(lineEnd.x, lineEnd.y, 0f);
			return;
		}
		double b = c1 / c2;
		float px = lineStart.x + (float) (b * vx);
		float py = lineStart.y + (float) (b * vy);
		result.set(px, py, 0f);
		return;
	}

	/*
	 * Places into "result" vector the result of rotating initialP along with
	 * the line segment iA->iB as it turns into fA->fB. Results not
	 * guaranteed if lengths are unequal.
	 */
	public static void rotateRelative(Vector3d initialP, Vector3d iA,
			Vector3d iB, Vector3d fA, Vector3d fB, Vector3d result) {
		double rx = iB.x - iA.x;
		double ry = iB.y - iA.y;
		double rpx = fB.x - fA.x;
		double rpy = fB.y - fA.y;
		double sx = -ry;
		double sy = rx;
		double distsqr = rx * rx + ry * ry;
		double pminusax = initialP.x - iA.x;
		double pminusay = initialP.y - iA.y;
		double kx = pminusax / (distsqr);
		double ky = pminusay / (distsqr);
		double spx = -rpy;
		double spy = rpx;
		result.set((float) (fA.x + (kx * rx + ky * ry) * rpx + (kx * sx + ky
				* sy)
				* spx),
				(float) (fA.y + (kx * rx + ky * ry) * rpy + (kx * sx + ky * sy)
						* spy), 0f);
		// System.out.println(result.x+" "+result.y);
	}

	/*
	 * Returns proportional time of intersection between a point moving from
	 * l0->l1 and circle at c.
	 * 
	 * Returns -1.0f if no collision detected in range, otherwise returns a
	 * float between 0f and 1f.
	 * 
	 * Always finds first intersection, so order of l0/l1 matters!
	 * 
	 */
	public static float lineCircleIntersectTOC(Vector3d l0, Vector3d l1,
			Vector3d c, float rad) {
		/*
		 * Is there any way to do this without the square root? [probably not, I
		 * think]
		 */

		float vx = l0.x - c.x;
		float vy = l0.y - c.y;
		float wx = l1.x - l0.x;
		float wy = l1.y - l0.y;

		float vdotw = vx * wx + vy * wy;
		float vdotv = vx * vx + vy * vy;
		float wdotw = wx * wx + wy * wy;

		double disc = vdotw * vdotw - wdotw * (vdotv - rad * rad);
		if (disc < 0.0) {
			return -1.0f;
		}
		double rootDisc = Math.sqrt(disc);
		// We prefer the first root (with a smaller TOC)...
		double root = -(vdotw + rootDisc) / wdotw;
		if (root > 0.0 && root < 1.0) {
			return (float) root;
		}
		// ...though if it doesn't work for us, we'll try the other.
		root = -(vdotw - rootDisc) / wdotw;
		if (root > 0.0 && root < 1.0) {
			return (float) root;
		}
		return -1.0f;
	}

	/*
	 * Returns proportional time of collision between swept moving circle and
	 * moving line segment.
	 * 
	 * Circle moves from c0->c1 Line segment moves from (la0->lb0)->(la1->lb1)
	 * 
	 * Returns -1.0f if no collision detected in range, otherwise returns a
	 * float between 0f and 1f.
	 */
	public static float circleLineSegTOC(Vector3d c0, Vector3d c1,
			Vector3d la0, Vector3d lb0, Vector3d la1, Vector3d lb1, float rad) {
		/*
		 * There are two possible ways to do this: 1) Transform line to circle's
		 * reference frame, check for quad (swept line seg) touching circle. 2)
		 * Transform circle to line's reference frame, check for capsule (swept
		 * circle ) touching line seg.
		 * 
		 * Should test each way and see which one is faster...
		 * 
		 */

		float toc = -1.0f;

		return toc;
	}

	/*
	 * 2d circle/circle time of collision, proportional. If already penetrating
	 * at beginning of step, returns 0.0f. If no penetration in this range,
	 * returns -1.0f.
	 */
	public static float circleCircleTOC(Vector3d a0, Vector3d a1, Vector3d b0,
			Vector3d b1, float ra, float rb) {

		// Transform into rest frame of circle b
		bufferA.set(a0.x - b0.x + b1.x, a0.y - b0.y + b1.y, 0.0f);
		// From here on, b0 is irrelevant, and ta0 represents the transformed
		// value of a0
		
		//Add an early exit SAT test here, since the true test requires sqrt
		
		Vector3d ta0 = bufferA;
		float d0 = (b1.x - ta0.x) * (b1.x - ta0.x) + (b1.y - ta0.y)
				* (b1.y - ta0.y);
		float rad = ra + rb;
		if (d0 < rad * rad)
			return 0.0f; // already penetrating
		else
			return lineCircleIntersectTOC(ta0, a1, b1, rad);

	}

	public String toString() {
		return "" + x + "," + y + "," + z;
	}

}// end of com.rgbgame.game.Vector3d class
